﻿#include "kinematics.h"
#include <cmath>
#include <QDebug>

namespace CIRT_LBR {

Transform::Transform()
{
    nx = 1; ny = 0; nz = 0;
    ox = 0; oy = 1; oz = 0;
    ax = 0; ay = 0; az = 1;
    px = 0; py = 0; pz = 0;
}

Transform::Transform(double _nx, double _ny, double _nz, double _ox, double _oy, double _oz, double _ax, double _ay, double _az, double _px, double _py, double _pz)
    :nx(_nx),ny(_ny),nz(_nz),
      ox(_ox),oy(_oy),oz(_oz),
      ax(_ax),ay(_ay),az(_az),
      px(_px),py(_py),pz(_pz)
{

}

Transform::Transform(const Transform &T)
{
    nx = T.nx; ny = T.ny; nz = T.nz;
    ox = T.ox; oy = T.oy; oz = T.oz;
    ax = T.ax; ay = T.ay; az = T.az;
    px = T.px; py = T.py; pz = T.pz;
}

Transform& Transform::operator=(const Transform &T)
{
    nx = T.nx; ny = T.ny; nz = T.nz;
    ox = T.ox; oy = T.oy; oz = T.oz;
    ax = T.ax; ay = T.ay; az = T.az;
    px = T.px; py = T.py; pz = T.pz;
    return *this;
}

Transform& Transform::operator-=(const Transform &T)
{
    nx -= T.nx; ny -= T.ny; nz -= T.nz;
    ox -= T.ox; oy -= T.oy; oz -= T.oz;
    ax -= T.ax; ay -= T.ay; az -= T.az;
    px -= T.px; py -= T.py; pz -= T.pz;
    return *this;
}

double Transform::abs()
{
    double result = 0;
    result = std::fabs(nx)+std::fabs(ny)+std::fabs(nz)+
            std::abs(ox)+std::abs(oy)+std::fabs(oz)+
            std::abs(ax)+std::abs(ay)+std::fabs(az)+
            std::abs(px)+std::abs(py)+std::abs(pz);
    return result;
}

Transform Transform::operator*(const Transform &T)
{
    double a11,a21,a31,a12,a22,a32,a13,a23,a33,a14,a24,a34;
    a11 = nx*T.nx+ox*T.ny+ax*T.nz; a12 = nx*T.ox+ox*T.oy+ax*T.oz; a13 = nx*T.ax+ox*T.ay+ax*T.az;
    a21 = ny*T.nx+oy*T.ny+ay*T.nz; a22 = ny*T.ox+oy*T.oy+ay*T.oz; a23 = ny*T.ax+oy*T.ay+ay*T.az;
    a31 = nz*T.nx+oz*T.ny+az*T.nz; a32 = nz*T.ox+oz*T.oy+az*T.oz; a33 = nz*T.ax+oz*T.ay+az*T.az;
    a14 = nx*T.px+ox*T.py+ax*T.pz+px;
    a24 = ny*T.px+oy*T.py+ay*T.pz+py;
    a34 = nz*T.px+oz*T.py+az*T.pz+pz;
    return Transform(a11,a21,a31,a12,a22,a32,a13,a23,a33,a14,a24,a34);
}

void Transform::toeulerangle()
{
    double trace = nx+oy+az; //matrix.m[0][0] + matrix.m[1][1] + matrix.m[2][2];
    double pitch, yaw, roll,w,x,y,z;
    if (trace > 0) {
        pitch = acos(az / trace);//matrix.m[2][2] / trace
        yaw = atan2(oz,nz);//(matrix.m[1][2], matrix.m[0][2]);
        roll = atan2(ax,ay);//(matrix.m[2][0], matrix.m[2][1]);
        w = atan2(ay-oz, ox - ny) * 180/PI; // in degrees
        x = atan2(-ax, az) * 180/PI; // in degrees
        y = atan2(nz - ax, ay - oz) * 180/PI; // in degrees
        z = atan2(ox + ny, oy + nx) * 180/PI; // in degrees
    } else if((nx>oy)&&(nx>az)) {// ((matrix.m[0][0] > matrix.m[1][1]) && (matrix.m[0][0] > matrix.m[2][2])) {
        pitch = acos(nx/trace);//(matrix.m[0][0] / trace);
        yaw = atan2(-ox,oy);//(-matrix.m[1][0], matrix.m[1][1]);
        roll = atan2(ay,az);//(matrix.m[2][1], matrix.m[2][2]);
    } else if (ny>az) { // (matrix.m[1][1] > matrix.m[2][2]) {
        pitch = acos(oy/trace);//(matrix.m[1][1] / trace);
        yaw = atan2(ny,nx);//(matrix.m[0][1], matrix.m[0][0]);
        roll = atan2(-ax,az);//(-matrix.m[2][0], matrix.m[2][2]);
    } else {
        pitch = acos(az/trace); //(matrix.m[2][2] / trace);
        yaw = atan2(nz,nx);//(matrix.m[0][2], matrix.m[0][0]);
        roll = atan2(ox,oy);//(matrix.m[1][0], matrix.m[1][1]);
    }


    qDebug()<<"rpy is:["<<roll * 180/PI<<pitch * 180/PI<<yaw * 180/PI<<"]"<<w<<x<<y<<z;


    x = atan2(ay, az);
    y = atan2(-ax, sqrt(ay * ay + az * az));
    z = atan2(ox, nx);

    qDebug()<<x* 180/PI<<y* 180/PI<<z* 180/PI;
//    printf("rpy is:[ ");
//    printf("%lf ",roll * 180/PI);
//    printf("%lf ",pitch * 180/PI);
//    printf("%lf ",yaw * 180/PI);
//    printf("]\n");
    //eulerAngle.pitch = pitch * 180 / M_PI;  // 将弧度转换为角度
    //eulerAngle.yaw = yaw * 180 / M_PI;  // 将弧度转换为角度
    //eulerAngle.roll = roll * 180 / M_PI;  // 将弧度转换为角度
}

std::ostream & operator<<(std::ostream &out, const Transform &T)
{
    out<<T.nx<<" "<<T.ox<<" "<<T.ax<<" "<<T.px<<"\n"
      <<T.ny<<" "<<T.oy<<" "<<T.ay<<" "<<T.py<<"\n"
     <<T.nz<<" "<<T.oz<<" "<<T.az<<" "<<T.pz<<"\n";
    return out;
}

Transform LBR_fk(const QVector<double> &Q)
{
    double d1,a2,a3,d4,d5,d6;
    // parameters
    d1 = 0.1295;
    a2 = 0.442;
    a3 = 0.386;
    d4 = 0.124;
    d5 = 0.105;
    d6 = 0.097;

    double q1,q2,q3,q4,q5,q6;
    q1 = Q[0]; q2 = Q[1]; q3 = Q[2]; q4 = Q[3]; q5 = Q[4]; q6 = Q[5];

    double sinq1,sinq2,sinq5,sinq6,sinq23,sinq234;
    double cosq1,cosq2,cosq5,cosq6,cosq23,cosq234;
    sinq1 = sin(q1); cosq1 = cos(q1);
    sinq2 = sin(q2); cosq2 = cos(q2);
    sinq5 = sin(q5); cosq5 = cos(q5);
    sinq6 = sin(q6); cosq6 = cos(q6);
    sinq23 = sin(q2+q3); cosq23 = cos(q2+q3);
    sinq234 = sin(q2+q3+q4); cosq234 = cos(q2+q3+q4);

    double nx,ny,nz,ox,oy,oz,ax,ay,az,px,py,pz;
    nx = cosq1*cosq234*cosq5+sinq1*sinq5;
    ny = sinq1*cosq234*cosq5-cosq1*sinq5;
    nz = sinq234*cosq5;
    ox = -cosq1*cosq234*sinq5+sinq1*cosq5;
    oy = -sinq1*cosq234*sinq5-cosq1*cosq5;
    oz = -sinq234*sinq5;
    ax = cosq1*sinq234;
    ay = sinq1*sinq234;
    az = -cosq234;
    px = cosq1*sinq234*d5+cosq1*cosq23*a3+sinq1*d4+cosq1*cosq2*a2;
    py = sinq1*sinq234*d5+sinq1*cosq23*a3-cosq1*d4+sinq1*cosq2*a2;
    pz = -cosq234*d5+sinq23*a3+sinq2*a2+d1;

    double a11,a21,a31,a12,a22,a32,a13,a23,a33,a14,a24,a34;
    a11 = nx*cosq6-ax*sinq6;
    a21 = ny*cosq6-ay*sinq6;
    a31 = nz*cosq6-az*sinq6;
    a12 = -nx*sinq6-ax*cosq6;
    a22 = -ny*sinq6-ay*cosq6;
    a32 =  -nz*sinq6-az*cosq6;
    a13 = ox;
    a23 = oy;
    a33 = oz;
    a14 = d6*ox+px;
    a24 = d6*oy+py;
    a34 = d6*oz+pz;

    return Transform(a11,a21,a31,a12,a22,a32,a13,a23,a33,a14,a24,a34);
}

QVector<QVector<double> > LBR_ik(const Transform &T)
{
    double d1,a2,a3,d4,d5,d6;
    // parameters
    d1 = 0.1295;
    a2 = 0.442;
    a3 = 0.386;
    d4 = 0.124;
    d5 = 0.105;
    d6 = 0.097;
    double offset[6] = { -90,90,0,90,0,-90 };
    double nx,ny,nz,ox,oy,oz,ax,ay,az,px,py,pz;
    nx = T.nx; ny = T.ny; nz = T.nz;
    ox = T.ox; oy = T.oy; oz = T.oz;
    ax = T.ax; ay = T.ay; az = T.az;
    px = T.px; py = T.py; pz = T.pz;

    QVector<QVector<double> > Q;
    QVector<double> q_last,q_current(6,0);
    double q1,q2,q3,q234,q4,q5,q6;
    double sinq1,sinq2,sinq3,sinq5,sinq6,sinq234;
    double cosq1,cosq2,cosq3,cosq5,cosq6,cosq234;

    // ---------- solve q1 ----------
    std::vector<std::pair<double, double> > q1_sin_cos_pair;
    q1_sin_cos_pair = phase_shift_eq(-px+d6*ax,py-d6*ay,-d4);
    //std::cout<<"q1 num: "<<q1_sin_cos_pair.size()<<std::endl;
    if(!q1_sin_cos_pair.size())
    {
        std::cout<<"no q1 exists"<<std::endl;
        return Q;
    }
    double h1,h2,h3,h4;
    h1 = a3+d5-a2;
    h2 = a2+d5-a3;
    h3 = a2+a3-d5;
    h4 = a2+a3+d5;

    for(unsigned int i=0;i<q1_sin_cos_pair.size();++i)
    {
        sinq1 = q1_sin_cos_pair[i].first;
        cosq1 = q1_sin_cos_pair[i].second;

        std::vector<std::pair<double, double> > q5_sin_cos_pair;
        // --------- solve q5 ---------
        cosq5 = (-d4+px*sinq1-py*cosq1)/d6;
        sinq5 = 0;
        if(std::fabs(std::fabs(cosq5)-1)>1e-6)
        {
            std::cout<<"cosq5 not close to -1 or 1"<<std::endl;
            if(cosq5>1||cosq5<-1)
            {
                std::cout<<"cosq5 exceeds range [-1, 1]"<<std::endl;
                std::cout<<"no q5 exists"<<std::endl;
                continue;
            }
            q5_sin_cos_pair.push_back(std::make_pair(std::sqrt(1-cosq5*cosq5),cosq5));
            q5_sin_cos_pair.push_back(std::make_pair(-std::sqrt(1-cosq5*cosq5),cosq5));
        }
        else{
            q5_sin_cos_pair.push_back(std::make_pair(0,cosq5));
        }

        double p15x,p15z,p15;
        p15x = cosq1*(px-d6*ax)+sinq1*(py-d6*ay);
        p15z = pz-d1-d6*az;
        p15 = p15x*p15x + p15z*p15z;
        double alpha,beta,M,N;

        alpha = cosq1*nx+sinq1*ny;
        beta = cosq1*ox+sinq1*oy;
        M = p15x*alpha+p15z*nz;
        N = p15x*beta+p15z*oz;

        for(unsigned int j=0;j<q5_sin_cos_pair.size();++j)
        {
            // ------- solve q6 --------
            sinq5 = q5_sin_cos_pair[j].first;
            cosq5 = q5_sin_cos_pair[j].second;

            std::cout<<"sinq5: "<<sinq5<<std::endl;

            std::vector<std::pair<double,double> > q6_sin_cos_pair;

            if(std::fabs(std::fabs(cosq5)-1)<1e-6) // joint 2,3,4,6 parallel
            {
                std::cout<<"--- 2,3,4,6 joints paraller ---"<<std::endl;

                // ------ range check -------
                double delta_upper = p15-h4*h4;
                double delta_lower = p15-h1*h1;
                if(delta_upper>1e-6)
                {
                    std::cout<<"delta_upper: "<<delta_upper<<std::endl;
                    std::cout<<"no solution, strech out of range upper"<<std::endl;
                    continue;
                }

                if(delta_lower<-1e-6)
                {
                    std::cout<<"delta_lower: "<<delta_lower<<std::endl;
                    std::cout<<"no solution, strech out of range lower";
                    continue;
                }

                //                if(p15>h2*h2)
                //                {
                //                    if(p15<h3*h3) // ----- middle segment -----
                //                    {
                //                        std::cout<<"--- middle segment ---"<<std::endl;
                //                        q6_sin_cos_pair = phase_shift_eq(M/std::sqrt(p15),N/std::sqrt(p15),(std::sqrt(p15)-a2-a3+d5)/(a3-d5)+1);
                //                    }
                //                    else    // ----- right segment -----
                //                    {
                //                        std::cout<<"--- right segment ---"<<std::endl;
                //                        q6_sin_cos_pair.push_back(std::make_pair<double,double>(M/std::sqrt(p15),N/std::sqrt(p15)));
                //                    }

                //                }
                //                else     // ----- left segment -----
                //                {
                //                    std::cout<<"--- left segment ---"<<std::endl;
                //                    q6_sin_cos_pair.push_back(std::make_pair<double,double>(-M/std::sqrt(p15),-N/std::sqrt(p15)));
                //                }
                double sqrt_p15 = std::sqrt(p15);
                q6_sin_cos_pair = phase_shift_eq(M/sqrt_p15,N/sqrt_p15,-(sqrt_p15+a2-a3-d5)/a2+1);


            }
            else   // 2,3,4,6 joints not parallel
            {
                std::cout<<"2,3,4,6, joints not parallel"<<std::endl;
                q6_sin_cos_pair.push_back(std::make_pair<double,double>(-(ox*sinq1-oy*cosq1)/sinq5,-(-nx*sinq1+ny*cosq1)/sinq5));
            }
            std::cout<<"q6 num "<<q6_sin_cos_pair.size()<<std::endl;
            // solve q2
            for(unsigned int k=0;k<q6_sin_cos_pair.size();++k)
            {
                sinq6 = q6_sin_cos_pair[k].first;
                cosq6 = q6_sin_cos_pair[k].second;
                std::cout<<"sinq6: "<<sinq6<<std::endl;
                std::cout<<"cosq6: "<<cosq6<<std::endl;
                // solve q2
                sinq234 = -alpha*sinq6-beta*cosq6;
                cosq234 = nz*sinq6+oz*cosq6;
                double A,B;
                A = p15x-sinq234*d5;
                B = p15z+cosq234*d5;
                std::vector<std::pair<double,double> > q2_sin_cos_pair;
                q2_sin_cos_pair = phase_shift_eq(2*a2*B,2*a2*A,A*A+B*B+a2*a2-a3*a3);
                if(!q2_sin_cos_pair.size())
                {
                    std::cout<<"no q2 exists"<<std::endl;
                    continue;
                }
                std::cout<<"q2 num "<<q2_sin_cos_pair.size()<<std::endl;
                for(unsigned int l=0;l<q2_sin_cos_pair.size();++l)
                {
                    sinq2 = q2_sin_cos_pair[l].first;
                    cosq2 = q2_sin_cos_pair[l].second;
                    sinq3 = (B*cosq2-A*sinq2)/a3;
                    cosq3 = (A*A+B*B-a2*a2-a3*a3)/2/a2/a3;
                    q234 = std::atan2(sinq234,cosq234);
                    q2 = std::atan2(sinq2,cosq2);
                    q3 = std::atan2(sinq3,cosq3);
                    q4 = q234-q2-q3;
                    if(q4<-PI)
                    {
                        q4+=2*PI;
                    }
                    if(q4>PI)
                    {
                        q4-=2*PI;
                    }
                    q1 = std::atan2(sinq1,cosq1);
                    q5 = std::atan2(sinq5,cosq5);
                    q6 = std::atan2(sinq6,cosq6);
                    q_current[0]=q1*180/PI - offset[0];
                    q_current[1]=q2*180/PI - offset[1];
                    q_current[2]=q3*180/PI - offset[2];
                    q_current[3]=q4*180/PI - offset[3];
                    q_current[4]=q5*180/PI - offset[4];
                    q_current[5]=q6*180/PI - offset[5];
                    if(q_last.size())
                    {
                        if(!isequal(q_last,q_current))
                        {
                            Q.push_back(q_current);
                            q_last = q_current;
                            std::cout<<"got one solution"<<std::endl;
                        }
                    }
                    else
                    {
                        Q.push_back(q_current);
                        q_last = q_current;
                        std::cout<<"got one solution"<<std::endl;
                    }
                }
            }
        }
    }
    std::cout<<"LBR ik done"<<std::endl;
    return Q;
}

std::vector<std::pair<double, double> > phase_shift_eq(const double& a,const double& b, const double& c)
{
    std::vector<std::pair<double, double> > sin_cos_pair;
    double denominator = a*a+b*b;
    double delta = denominator-c*c;

    if(std::abs(delta)<1e-6)
    {
        double sinx = a*c/denominator;
        double cosx = b*c/denominator;
        sincosround(sinx); sincosround(cosx);
        sin_cos_pair.push_back(std::make_pair(sinx,cosx));
        return sin_cos_pair;
    }

    if(delta<0)
    {
        return sin_cos_pair;
    }

    double sinx1,cosx1,sinx2,cosx2;
    sinx1 = (a*c+b*std::sqrt(delta))/denominator;
    cosx1 = (b*c-a*std::sqrt(delta))/denominator;
    sinx2 = (a*c-b*std::sqrt(delta))/denominator;
    cosx2 = (b*c+a*std::sqrt(delta))/denominator;
    sincosround(sinx1); sincosround(cosx1); sincosround(sinx2); sincosround(cosx2);
    sin_cos_pair.push_back(std::make_pair(sinx1,cosx1));
    sin_cos_pair.push_back(std::make_pair(sinx2,cosx2));

    return sin_cos_pair;
}

bool isequal(const QVector<double> &a, const QVector<double> &b)
{
    double sum;
    for(unsigned int i=0;i<a.size();++i)
    {
        sum+=std::abs(a[i]-b[i]);
    }
    if(sum<1e-6)
    {
        return true;
    }

    return false;
}

void sincosround(double &value)
{
    if(std::fabs(value-1)<1e-6)
    {
        value = 1;
        return;
    }
    if(std::fabs(value+1)<1e-6)
    {
        value = -1;
        return;
    }
    if(std::fabs(value)<1e-6)
    {
        value = 0;
        return;
    }
}

double maxdistance(const QVector<double>& joint1, const QVector<double>& joint2)//joint2，joint1 中为角度
{
    double dist = 0;
    for (int i = 0; i < joint1.size() - 1; ++i)
    {
        double delta = joint1.at(i) - joint2.at(i);
        if (delta >= 360)
        {
            delta -= 360;
        }
        else if (delta <= -360)
        {
            delta += 360;
        }
        dist += qAbs(delta);
    }
    return dist;
}

QVector<float> nearstsloution(const QVector<QVector<double> > sluts, const QVector<double> current)
{
    QVector<float> slot;
    if(sluts.size()==0)
    {
        qDebug()<< "no slutions found about ";
        return {};
    }
    else
    {
        qDebug()<<"select sloution............"<<sluts;
        double del=maxdistance(current,sluts.at(0));

        int index=0;
        for(int i=1;i<sluts.size();++i)
        {
            double maxdist=maxdistance(current,sluts.at(i));
            //qDebug()<<"maxdist "<<i<<"=="<<maxdist;
            if(maxdist<del)
            {
                del=maxdist;
                index=i;
            }

        }
        //qDebug()<<"last index =="<<index<<del;
        if(del>180)
            return {};
        for(int j=0;j<6;++j)
        {
            slot.push_back(sluts.at(index).at(j));
        }
    }
    return slot;
}
int SIGN(double x)
{
    return (x > 0) - (x < 0);
}

QVector<QVector<double> > LBR_inverse(const Transform &T, double *q_sols, double *q)
{
    double d1 = 0.1295;
    double a2 = 0.442;
    double a3 = 0.386;
    double d4 = 0.124;
    double d5 = 0.105;
    double d6 = 0.097;
    QVector<QVector<double> > slout;
    const double ZERO_THRESH = 0.00000001;
    int num_sols = 0;
    double nx,ny,nz,ox,oy,oz,ax,ay,az,px,py,pz;
    nx = T.nx;  ox = T.ox;  ax = T.ax;  px = T.px;
    ny = T.ny;  oy = T.oy;  ay = T.ay;  py = T.py;
    nz = T.nz;  oz = T.oz;  az = T.az;  pz = T.pz;
    double qq1, qq2, qq3, qq4, qq5, qq6;


    double q1[2];
    {
        double A = d6 * ay - py;
        double B = d6 * ax - px;
        double R = A * A + B * B;
        if (fabs(A) < ZERO_THRESH) {
            double div;
            if (fabs(fabs(d4) - fabs(B)) < ZERO_THRESH)
                div = -SIGN(d4) * SIGN(B);
            else
                div = -d4 / B;
            double arcsin = asin(div);
            if (fabs(arcsin) < ZERO_THRESH)
                arcsin = 0.0;
            if (arcsin < 0.0)
                q1[0] = arcsin + 2.0 * PI;
            else
                q1[0] = arcsin;
            q1[1] = PI - arcsin;
        }
        else if (fabs(B) < ZERO_THRESH) {
            double div;
            if (fabs(fabs(d4) - fabs(A)) < ZERO_THRESH)
                div = SIGN(d4) * SIGN(A);
            else
                div = d4 / A;
            double arccos = acos(div);
            q1[0] = arccos;
            q1[1] = 2.0 * PI - arccos;
        }
        else if (d4 * d4 > R) {
            return slout;
        }
        else {
            double arccos = acos(d4 / sqrt(R));
            double arctan = atan2(A, B);
            double pos = arccos + arctan;
            double neg = -arccos + arctan;
            if (fabs(pos) < ZERO_THRESH)
                pos = 0.0;
            if (fabs(neg) < ZERO_THRESH)
                neg = 0.0;
            if (pos >= 0.0)
                q1[0] = pos;
            else
                q1[0] = 2.0 * PI + pos;
            if (neg >= 0.0)
                q1[1] = neg;
            else
                q1[1] = 2.0 * PI + neg;
        }

        qq1 = q1[1];
    }

    double q5[2][2];
    {
        for (int i = 0; i < 2; i++) {
            qDebug()<<q1[i]*RAD2DEG;
            double numer = (px * sin(q1[i]) - py * cos(q1[i]) - d4);
            double div;
            if (fabs(fabs(numer) - fabs(d6)) < ZERO_THRESH)
                div = SIGN(numer) * SIGN(d6);
            else
                div = numer / d6;
            double arccos = acos(div);
            q5[i][0] = arccos;
            q5[i][1] = 2.0 * PI - arccos;
        }

        qq5 = q5[1][0];
    }

    {
        for (int i = 0; i < 2; i++) {
            for (int j = 0; j < 2; j++) {
                double c1 = cos(q1[i]), s1 = sin(q1[i]);
                double c5 = cos(q5[i][j]), s5 = sin(q5[i][j]);
                double q6;

                if (fabs(s5) < ZERO_THRESH)
                    q6 = 0;
                else {
                    q6 = atan2(SIGN(s5) * -(ox * s1 - oy * c1),
                               SIGN(s5) * (nx * s1 - ny * c1));
                    if (fabs(q6) < ZERO_THRESH)
                        q6 = 0.0;
                    if (q6 < 0.0)
                        q6 += 2.0 * PI;
                }
                double q2[2], q3[2], q4[2];

                double c6 = cos(q6), s6 = sin(q6);
                double x04x = -s5 * (ax * c1 + ay * s1) - c5 * (s6 * (ox * c1 + oy * s1) - c6 * (nx * c1 + ny * s1));
                double x04y = c5 * (nz * c6 - oz * s6) - az * s5;
                double p13x = d5 * (s6 * (nx * c1 + ny * s1) + c6 * (ox * c1 + oy * s1)) - d6 * (ax * c1 + ay * s1) +
                              px * c1 + py * s1;
                double p13y = pz - d1 - d6 * az + d5 * (oz * c6 + nz * s6);

                double c3 = (p13x * p13x + p13y * p13y - a2 * a2 - a3 * a3) / (2.0 * a2 * a3);
                if (fabs(fabs(c3) - 1.0) < ZERO_THRESH)
                    c3 = SIGN(c3);
                else if (fabs(c3) > 1.0) {
                    // TODO NO SOLUTION
                    continue;
                }
                double arccos = acos(c3);
                q3[0] = arccos;
                q3[1] = 2.0 * PI - arccos;
                double denom = a2 * a2 + a3 * a3 + 2 * a2 * a3 * c3;
                double s3 = sin(arccos);
                double A = (a2 + a3 * c3), B = a3 * s3;
                q2[0] = atan2((A * p13y - B * p13x) / denom, (A * p13x + B * p13y) / denom);
                q2[1] = atan2((A * p13y + B * p13x) / denom, (A * p13x - B * p13y) / denom);
                double c23_0 = cos(q2[0] + q3[0]);
                double s23_0 = sin(q2[0] + q3[0]);
                double c23_1 = cos(q2[1] + q3[1]);
                double s23_1 = sin(q2[1] + q3[1]);
                q4[0] = atan2(c23_0 * x04y - s23_0 * x04x, x04x * c23_0 + x04y * s23_0);
                q4[1] = atan2(c23_1 * x04y - s23_1 * x04x, x04x * c23_1 + x04y * s23_1);
                QVector<double> temp;
                temp<<q1[i]*RAD2DEG<<q2[0]*RAD2DEG<<q3[0]*RAD2DEG<<q4[0]*RAD2DEG<<q5[i][j]*RAD2DEG<<q6*RAD2DEG;
                qq6 = q6;
                qq2 = q2[0];
                qq3 = q3[0];
                qq4 = q4[0];
                slout.push_back(temp);
                num_sols++;
            }
        }
    }
    *q = qq1; q++; *q = qq2; q++; *q = qq3; q++; *q = qq4; q++; *q = qq5; q++; *q = qq6;
    return slout;
}

}
